#define LG_CORE

#include <math.h>

#include "lg_math.h"



static inline LGFloat
dot(LGVector* v1, LGVector* v2) {
	return (v1->x * v2->x) + (v1->y * v2->y) 
			+ (v1->z * v2->z) + (v1->w * v2->w);
}

static inline LGFloat
dot2(LGVector* v1, LGVector* v2) {
	return (v1->x * v2->x) + (v1->y * v2->y) + (v1->z * v2->z);
}

static inline void
cross(LGVector* res, LGVector* v) {
	LGFloat x = (res->y * v->z) - (res->z * res->y);
	LGFloat y = (res->z * v->x) - (res->x * v->z);
	LGFloat z = res->x * v->y - res->y * v->x;
	res->x = x; res->y = y; res->z = z;
}

static inline void
normalize(LGVector* res) {
	LGFloat magSq = (res->x * res->x)
		+ (res->y * res->y)
		+ (res->z * res->z);
	if (magSq > 0.0f) {
		LGFloat i = 1.0f / (LGFloat)sqrt(magSq);
		res->x *= i;
		res->y *= i;
		res->z *= i;
	}
}

static inline LGFloat
length(LGVector* res) {
	return (LGFloat)sqrt((res->x * res->x)
		+ (res->y * res->y)
		+ (res->z * res->z));
}

static inline void
add(LGVector* res, LGVector* v) {
	res->x += v->x;
	res->y += v->y;
	res->z += v->z;
}


static inline void
sub(LGVector* res, LGVector* v) {
	res->x -= v->x;
	res->y -= v->y;
	res->z -= v->z;
}


LG_API LGFloat
lgDotVector(LGVector* v1, LGVector* v2) {
	return dot2(v1, v2);
}

LG_API LGFloat
lgDotVector2D(LGVector* v1, LGVector* v2) {
	return (v1->x * v2->x) + (v1->y * v2->y);
}

LG_API void
lgCrossVector(LGVector* res, LGVector* v) {
	cross(res, v);
}

LG_API void
lgNormalizeVector(LGVector* res) {
	normalize(res);
}
LG_API void
lgNormalizeVector2D(LGVector* res) {
	LGFloat magSq = (res->x * res->x) + (res->y * res->y);
	if (magSq > 0.0f) {
		LGFloat i = 1.0f / (LGFloat)sqrt(magSq);
		res->x *= i;
		res->y *= i;
	}
}

LG_API LGFloat
lgLengthVector(LGVector* res) {
	return length(res);
}

LG_API LGFloat
lgLengthVector2D(LGVector* res) {
	return (LGFloat) sqrt((res->x * res->x)
						+ (res->y * res->y));
}

LG_API void
lgAddVector(LGVector* res, LGVector* add) {
	res->x += add->x;
	res->y += add->y;
	res->z += add->z;
}
LG_API void
lgAddVector2D(LGVector* res, LGVector* add) {
	res->x += add->x;
	res->y += add->y;
}

LG_API void
lgSubVector(LGVector* res, LGVector* sub) {
	res->x -= sub->x;
	res->y -= sub->y;
	res->z -= sub->z;
}

LG_API void
lgSubVector2D(LGVector* res, LGVector* sub) {
	res->x -= sub->x;
	res->y -= sub->y;
}


LG_API LGFloat
lgDistanceVector(LGVector* v1, LGVector* v2) {
	LGFloat dx = v1->x - v2->x;
	LGFloat dy = v1->y - v2->y;
	LGFloat dz = v1->z - v2->z;
	return (LGFloat)sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
LG_API LGFloat
lgDistanceVector2D(LGVector* v1, LGVector* v2) {
	LGFloat dx = v1->x - v2->x;
	LGFloat dy = v1->y - v2->y;
	return (LGFloat)sqrt((dx * dx) + (dy * dy));
}



LG_API void
lgIdentityMatrix(LGMatrix* res) {
	res->A.a11 = 1.0f; res->A.a12 =res->A.a13 = res->A.a14 = 0.f;
	res->A.a22 = 1.0f; res->A.a21 = res->A.a23 = res->A.a24 = 0.f;
	res->A.a33 = 1.0f; res->A.a31 = res->A.a32 = res->A.a34 = 0.f;
	res->A.a44 = 1.0f; res->A.a41 = res->A.a42 = res->A.a43 = 0.f;
}
LG_API void
lgIdentityMatrix2D(LGMatrix* res) {
	res->A.a11 = 1.0f; res->A.a12 = res->A.a13 = 0.f;
	res->A.a22 = 1.0f; res->A.a21 = res->A.a23 = 0.f;
	res->A.a33 = 1.0f; res->A.a31 = res->A.a32 = 0.f;
}

LG_API void
lgMulMatrix(LGMatrix* res, LGMatrix* mul) {
#define RA (res->A)
#define MV (mul->V)
	LGFloat a1, a2, a3, a4;
	LGVector RV[4] = {
		{RA.a11, RA.a12, RA.a13, RA.a14},
		{RA.a21, RA.a22, RA.a23, RA.a24},
		{RA.a31, RA.a32, RA.a33, RA.a34},
		{RA.a41, RA.a42, RA.a43, RA.a44},
	};
	a1 = dot(&RV[0], &MV[0]);
	a2 = dot(&RV[0], &MV[1]);
	a3 = dot(&RV[0], &MV[2]);
	a4 = dot(&RV[0], &MV[3]);
	RA.a11 = a1; RA.a12 = a2; RA.a13 = a3; RA.a14 = a4;
	a1 = dot(&RV[1], &MV[0]);
	a2 = dot(&RV[1], &MV[1]);
	a3 = dot(&RV[1], &MV[2]);
	a4 = dot(&RV[1], &MV[3]);
	RA.a21 = a1; RA.a22 = a2; RA.a23 = a3; RA.a24 = a4;
	a1 = dot(&RV[2], &MV[0]);
	a2 = dot(&RV[2], &MV[1]);
	a3 = dot(&RV[2], &MV[2]);
	a4 = dot(&RV[2], &MV[3]);
	RA.a31 = a1; RA.a32 = a2; RA.a33 = a3; RA.a34 = a4;
	a1 = dot(&RV[3], &MV[0]);
	a2 = dot(&RV[3], &MV[1]);
	a3 = dot(&RV[3], &MV[2]);
	a4 = dot(&RV[3], &MV[3]);
	RA.a41 = a1; RA.a42 = a2; RA.a43 = a3; RA.a44 = a4;
}

LG_API void
lgMulMatrix2D(LGMatrix* res, LGMatrix* mul) {
#define RA (res->A)
#define MA (mul->A)
#define RV (res->V)
	LGFloat a1, a2, a3;
	LGVector V[3] = {
		{MA.a11, MA.a12, MA.a13, 0.f},
		{MA.a21, MA.a22, MA.a23, 0.f},
		{MA.a31, MA.a32, MA.a33, 0.f},
	};
	a1 = dot(&V[0], &RV[0]);
	a2 = dot(&V[0], &RV[1]);
	a3 = dot(&V[0], &RV[2]);
	RA.a11 = a1; RA.a12 = a2; RA.a13 = a3;
	a1 = dot(&V[1], &RV[0]);
	a2 = dot(&V[1], &RV[1]);
	a3 = dot(&V[1], &RV[2]);
	RA.a21 = a1; RA.a22 = a2; RA.a23 = a3;
	a1 = dot(&V[2], &RV[0]);
	a2 = dot(&V[2], &RV[1]);
	a3 = dot(&V[2], &RV[2]);
	RA.a31 = a1; RA.a32 = a2; RA.a33 = a3;
}

LG_API void
lgMoveMatrix(LGMatrix* res, LGVector* v) {
#define RA (res->A)
	if (v->x != 0.0f) {
		RA.a11 += RA.a41 * v->x;
		RA.a12 += RA.a42 * v->x;
		RA.a13 += RA.a43 * v->x;
		RA.a14 += RA.a44 * v->x;
	}
	if (v->y != 0.0f) {
		RA.a21 += RA.a41 * v->y;
		RA.a22 += RA.a42 * v->y;
		RA.a23 += RA.a43 * v->y;
		RA.a24 += RA.a44 * v->y;
	}
	if (v->z != 0.0f) {
		RA.a31 += RA.a41 * v->z;
		RA.a32 += RA.a42 * v->z;
		RA.a33 += RA.a43 * v->z;
		RA.a34 += RA.a44 * v->z;
	}
}
LG_API void
lgMoveMatrix2D(LGMatrix* res, LGVec2* v) {
#define RA (res->A)
	if (v->x != 0.0f) {
		RA.a11 += RA.a41 * v->x;
		RA.a12 += RA.a42 * v->x;
		RA.a13 += RA.a43 * v->x;
		RA.a14 += RA.a44 * v->x;
	}
	if (v->y != 0.0f) {
		RA.a21 += RA.a41 * v->y;
		RA.a22 += RA.a42 * v->y;
		RA.a23 += RA.a43 * v->y;
		RA.a24 += RA.a44 * v->y;
	}
}

LG_API void
lgScaleMatrix(LGMatrix* res, LGVector* v) {
#define RA (res->A)
	if (v->x != 1.0f) {
		RA.a11 *= v->x;
		RA.a12 *= v->x;
		RA.a13 *= v->x;
		RA.a14 *= v->x;
	}
	if (v->y != 1.0f) {
		RA.a21 *= v->y;
		RA.a22 *= v->y;
		RA.a23 *= v->y;
		RA.a24 *= v->y;
	}
	if (v->z != 1.0f) {
		RA.a31 *= v->z;
		RA.a32 *= v->z;
		RA.a33 *= v->z;
		RA.a34 *= v->z;
	}
}
LG_API void
lgScaleMatrix2D(LGMatrix* res, LGVec2* v) {
#define RA (res->A)
	if (v->x != 1.0f) {
		RA.a11 *= v->x;
		RA.a12 *= v->x;
		RA.a13 *= v->x;
		RA.a14 *= v->x;
	}
	if (v->y != 1.0f) {
		RA.a21 *= v->y;
		RA.a22 *= v->y;
		RA.a23 *= v->y;
		RA.a24 *= v->y;
	}
}

static void
rotateMatrixToX(LGMatrix* res, LGFloat radian) {
	LGFloat c = (LGFloat)lgCos(radian);
	LGFloat s = (LGFloat)lgSin(radian);

	LGFloat v21 = res->A.a21, v22 = res->A.a22, v23 = res->A.a23, v24 = res->A.a24;
	LGFloat v31 = res->A.a31, v32 = res->A.a32, v33 = res->A.a33, v34 = res->A.a34;
	res->A.a21 = v21 * c + v31 * (-s);
	res->A.a22 = v22 * c + v32 * (-s);
	res->A.a23 = v23 * c + v33 * (-s);
	res->A.a24 = v24 * c + v34 * (-s);

	res->A.a31 = v21 * s + v31 * c;
	res->A.a32 = v22 * s + v32 * c;
	res->A.a33 = v23 * s + v33 * c;
	res->A.a34 = v24 * s + v34 * c;
}

static void
rotateMatirxToY(LGMatrix* res, LGFloat radian) {
	LGFloat c = (LGFloat)lgCos(radian);
	LGFloat s = (LGFloat)lgSin(radian);

	LGFloat v11 = res->A.a11, v12 = res->A.a12, v13 = res->A.a13, v14 = res->A.a14;
	LGFloat v31 = res->A.a31, v32 = res->A.a32, v33 = res->A.a33, v34 = res->A.a34;
	res->A.a11 = v11 * c + v31 * s;
	res->A.a12 = v12 * c + v32 * s;
	res->A.a13 = v13 * c + v33 * s;
	res->A.a14 = v14 * c + v34 * s;
	res->A.a31 = v11 * (-s) + v31 * c;
	res->A.a32 = v12 * (-s) + v32 * c;
	res->A.a33 = v13 * (-s) + v33 * c;
	res->A.a34 = v14 * (-s) + v34 * c;
}

static void
rotateMatrixToZ(LGMatrix* res, LGFloat radian) {
	LGFloat c = (LGFloat)lgCos(radian);
	LGFloat s = (LGFloat)lgSin(radian);

	LGFloat v11 = res->A.a11, v12 = res->A.a12, v13 = res->A.a13, v14 = res->A.a14;
	LGFloat v21 = res->A.a21, v22 = res->A.a22, v23 = res->A.a23, v24 = res->A.a24;
	res->A.a11 = v11 * c - v21 * s;
	res->A.a12 = v12 * c - v22 * s;
	res->A.a13 = v13 * c - v23 * s;
	res->A.a14 = v14 * c - v24 * s;
	res->A.a21 = v11 * s + v21 * c;
	res->A.a22 = v12 * s + v22 * c;
	res->A.a23 = v13 * s + v23 * c;
	res->A.a24 = v14 * s + v24 * c;
}

LG_API void
lgRotateMatrix(LGMatrix* res, LGVector* v) {
	if (v->x) {
		rotateMatrixToX(res, v->x);
	}
	if (v->y) {
		rotateMatirxToY(res, v->y);
	}
	if (v->z) {
		rotateMatrixToZ(res, v->z);
	}
}
LG_API void
lgRotateMatrix2D(LGMatrix* res, LGFloat radian) {
	rotateMatrixToZ(res, radian);
}

LG_API void
lgRotateMatrixV(LGMatrix* res, LGVector* v, LGFloat radian) {
	LGVector axis = *v;
	normalize(&axis);
	LGFloat c = (LGFloat)lgCos(radian);
	LGFloat s = (LGFloat)lgSin(radian);
	LGFloat xx = axis.x * axis.x;
	LGFloat yy = axis.y * axis.y;
	LGFloat zz = axis.z * axis.z;
	LGFloat xy = axis.x * axis.y;
	LGFloat xz = axis.x * axis.z;
	LGFloat yz = axis.y * axis.z;
	LGMatrix r;
	lgIdentityMatrix(&r);
	r.A.a11 = c + xx * (1 - c);
	r.A.a12 = xy * (1 - c) - axis.z * s;
	r.A.a13 = xz * (1 - c) + axis.y * s;
	r.A.a21 = xy * (1 - c) + axis.z * s;
	r.A.a22 = c + yy * (1 - c);
	r.A.a23 = yz * (1 - c) - axis.x * s;
	r.A.a31 = xz * (1 - c) - axis.y * s;
	r.A.a32 = yz * (1 - c) + axis.x * s;
	r.A.a33 = c + zz * (1 - c);
	lgMulMatrix(res, &r);
}


LG_API void
lgPerspectiveMatrix(LGMatrix* res,
	LGFloat fov, LGFloat aspect, LGFloat near, LGFloat far) {
	lgIdentityMatrix(res);
	//LGFloat a = (1.0f / (LGFloat)tan(fov * 0.5f));
	LGFloat a = (LGFloat)tan(fov / 2.0f);
	res->A.a11 = 1.0f / (aspect * a);
	res->A.a22 = 1.0f / a;
	
	//res->A.a33 = -(near + far) / (far - near);
	res->A.a33 = -(far + near) / (far - near);

	res->A.a34 = - ((2 * near * far) / (far - near));
	//res->A.a34 = 1.0f;
	
	//res->A.a43 = -(2.0f * near * far) / (far - near);
	res->A.a43 = -1;
	//res->A.a44 = 0;

	//tan_half_angle = tan(angle / 2);
	//mat_set(to_return, 1, 1, 1 / (ratio * tan_half_angle));
	//mat_set(to_return, 2, 2, 1 / (tan_half_angle));
	//mat_set(to_return, 3, 3, -(far + near) / (far - near));
	//mat_set(to_return, 4, 3, -1);
	//mat_set(to_return, 3, 4, -(2 * far * near) / (far - near));
}

LG_API void
lgOrthoMatrix(LGMatrix* res, LGFloat left, LGFloat right,
		LGFloat bottom, LGFloat top, LGFloat near, LGFloat far) {
	res->A.a11 = 2.0f / (right - left);
	res->A.a14 = -((right + left) / (right - left));
	res->A.a22 = 2.0f / (top / bottom);
	res->A.a24 = -((top + bottom) / (top - bottom));
	res->A.a33 = -2.0f / (far - near);
	res->A.a34 = -((far + near) / (far - near));
}

LG_API void
lgVectorMulMatrix(LGVector* res, LGMatrix* mul) {
#define MA (mul->A)
#define MV (mul->V)
	LGFloat x = res->x * MA.a11 + res->y * MA.a12 + res->z * MA.a13 + res->w * MA.a14;
	LGFloat y = res->x * MA.a21 + res->y * MA.a22 + res->z * MA.a23 + res->w * MA.a24;
	LGFloat z = res->x * MA.a31 + res->y * MA.a32 + res->z * MA.a33 + res->w * MA.a34;
	LGFloat w = res->x * MA.a41 + res->y * MA.a42 + res->z * MA.a43 + res->w * MA.a44;
	//LGFloat x = dot(res, &MV[0]);
	//LGFloat y = dot(res, &MV[1]);
	//LGFloat z = dot(res, &MV[2]);
	//LGFloat w = dot(res, &MV[3]);
	res->x = x; res->y = y; res->z = z; res->w = w;
}
LG_API void
lgVectorMulMatrixW(LGVector* res, LGMatrix* m, LGFloat w) {
	LGFloat x = res->x * m->A.a11 + res->y * m->A.a12 + res->z * m->A.a13 + w * m->A.a14;
	LGFloat y = res->x * m->A.a21 + res->y * m->A.a22 + res->z * m->A.a23 + w * m->A.a24;
	LGFloat z = res->x * m->A.a31 + res->y * m->A.a32 + res->z * m->A.a33 + w * m->A.a34;
	res->x = x; res->y = y; res->z = z;
}

LG_API void
lgVectorMulMatrix2D(LGVector* res, LGMatrix* m) {
	LGFloat x;
	x = res->x * m->A.a11 + res->y * m->A.a12;
	res->y = res->x * m->A.a21 + res->y * m->A.a22;
	res->x = x;
}
LG_API void
lgVectorMulMatrix2DW(LGVector* res, LGMatrix* m, LGFloat w) {
	LGFloat x;
	x = res->x * m->A.a11 + res->y * m->A.a12 + w * m->A.a13;
	res->y = res->x * m->A.a21 + res->y * m->A.a22 + w * m->A.a23;
	res->x = x;
}

LG_API LGFloat
lgRadians(LGFloat degrees) {
	return degrees * (LG_PI / 180.0f);
}

LG_API LGFloat
lgDegrees(LGFloat radians) {
	return radians * (180.0f / LG_PI);
}

LG_API void
lgLookAt(LGMatrix* res, LGVector* eye, LGVector* center, LGVector* up){

	//vec<3, T, Q> const f(normalize(center - eye));
	//vec<3, T, Q> const s(normalize(cross(f, up)));
	//vec<3, T, Q> const u(cross(s, f));
	//mat<4, 4, T, Q> Result(1);
	//Result[0][0] = s.x;
	//Result[1][0] = s.y;
	//Result[2][0] = s.z;
	//Result[0][1] = u.x;
	//Result[1][1] = u.y;
	//Result[2][1] = u.z;
	//Result[0][2] = -f.x;
	//Result[1][2] = -f.y;
	//Result[2][2] = -f.z;
	//Result[3][0] = -dot(s, eye);
	//Result[3][1] = -dot(u, eye);
	//Result[3][2] = dot(f, eye);
	//return Result;

	LGVector f = *center;
	lgSubVector(&f, eye);
	lgNormalizeVector(&f);
	LGVector s = f;
	lgCrossVector(&s, up);
	LGVector u = s;
	lgCrossVector(&u, &f);

	//res->A.a11 = s.x;
	res->A.a12 = s.y;
	res->A.a13 = s.z;
	res->A.a21 = u.x;
	//res->A.a22 = u.y;
	res->A.a23 = u.z;
	res->A.a31 = -f.x;
	res->A.a32 = -f.y;
	//res->A.a33 = -f.z;
	res->A.a14 = -lgDotVector(&s, eye);
	res->A.a24 = -lgDotVector(&u, eye);
	res->A.a34 = lgDotVector(&f, eye);


	//res->A.a31 += res->A.a41 * v->z;
	//res->A.a32 += res->A.a42 * v->z;
	//res->A.a33 += res->A.a43 * v->z;
	//res->A.a34 += res->A.a44 * v->z;
}
